#include "hardwareDef.h"

#include "IBUS.h"
#include "JY61P.h"
#include "AOAI.h"
#include "IMU.h"
#include "DOG.h"
#include "MIXER.h"
#include "RCPWM.h"
#include "simpleMath.h"

uint8_t calcState = 0;//计算阶段标志
bool calcFlag = 0;//计算更新标志
uint8_t flightMode;//飞行模式，0失控或自稳模式，1手动模式，2攻角模式，3攻角速率双闭环模式
int32_t input[4];//输入数据，横滚0、俯仰1、油门2、偏航3
int32_t ctrl[4];//横滚0、俯仰1、油门2、偏航3
int32_t feedback[5] = {};//横滚速率0、俯仰速率1、AOA 2、横滚角3、俯仰角4
int32_t mid;//中间控制值

void PID_ROL(void)
{
  static int32_t out, e_1, e_2;//建立输出静态变量、上次误差、上上次误差静态变量
  int32_t e;
  switch (flightMode)
  {//根据飞行模式调整PID策略
  case 0://自稳模式-ROLLPID-ROLLPID-ROLLPID-ROLLPID-ROLLPID-ROLLPID-ROLLPID-ROLLPID-ROLLPID-ROLLPID-ROLLPID-ROLLPID-ROLLPID-ROLLPID-ROLLPID
    //输入数据，横滚0、俯仰1、油门2、偏航3//横滚速率0、俯仰速率1、AOA 2、横滚角3、俯仰角4
    //横滚自稳模式，输入为input0，反馈为横滚角3，不需要使用比率缩放
    e = input[0] - feedback[3];//使用输入0和反馈3计算误差
    //P：0.6，I:0.0，D:0.0
    out = clamp(out + (e - e_1) * 3 / 5 + e * 0 + (e - 2 * e_1 + e_2) * 0, -4096, 4096);//使用增量式PID计算输出值
    e_2 = e_1; e_1 = e;//历史误差递归
    //横滚0、俯仰1、油门2、偏航3
    ctrl[0] = out;//横滚控制值赋值为输出值
    return;//返回

  case 1://手动模式
    //输入数据，横滚0、俯仰1、油门2、偏航3//横滚速率0、俯仰速率1、AOA 2、横滚角3、俯仰角4
    //横滚手动模式，输入为input0，不需要计算，直接输出
    out = input[0];//输出值为输入值
    e_2 = e_1; e_1 = 0;//误差递归，上次误差归零
    //横滚0、俯仰1、油门2、偏航3
    ctrl[0] = out;//横滚控制值赋值为输出值
    return;//返回

  default://攻角或双闭环模式-ROLLRATEPID-ROLLRATEPID-ROLLRATEPID-ROLLRATEPID-ROLLRATEPID-ROLLRATEPID-ROLLRATEPID-ROLLRATEPID-ROLLRATEPID
    //输入数据，横滚0、俯仰1、油门2、偏航3//横滚速率0、俯仰速率1、AOA 2、横滚角3、俯仰角4
    //横滚速率模式，输入为input0，反馈为横滚速率，不需要使用比例缩放
    e = input[0] - feedback[0];//使用输入0和反馈0计算误差
    //P:0.33，I:0.05，D:0.01
    out = clamp(out + (e - e_1) / 3 + e / 9  + (e - 2 * e_1 + e_2) / 45, -4096, 4096);//使用增量式PID计算输出值
    e_2 = e_1; e_1 = e;//历史误差递归
    //横滚0、俯仰1、油门2、偏航3
    ctrl[0] = out;//横滚控制值赋值为输出值
    return;//返回
  }
}

void PID_PIT(void)
{
  static int32_t out, e_1, e_2;//建立输出静态变量、上次误差、上上次误差静态变量
  int32_t e;
  switch (flightMode)
  {//根据飞行模式调整PID策略
  case 0://自稳模式-PITCHPID-PITCHPID-PITCHPID-PITCHPID-PITCHPID-PITCHPID-PITCHPID-PITCHPID-PITCHPID-PITCHPID-PITCHPID-PITCHPID-PITCHPID
    //输入数据，横滚0、俯仰1、油门2、偏航3//横滚速率0、俯仰速率1、AOA 2、横滚角3、俯仰角4
    //俯仰自稳模式，输入为input1，反馈为俯仰角4，使用比率缩放，将正输入值缩放到pRate倍
    if (input[1] > 0)
    {//如果输入1大于0
      e = input[1] * pRate - feedback[4];//将输入1放大到pRate倍，随后根据反馈4计算误差
    }
    else
    {//如果小于等于0
      e = input[1] - feedback[4];//直接根据输入1和反馈4计算误差
    }
    //P：0.6，I:0.0，D:0.0
    out = clamp(out + (e - e_1) * 3 / 5 + e * 0 + (e - 2 * e_1 + e_2) * 0, -4096, 4096);//使用增量式PID计算输出值
    e_2 = e_1; e_1 = e;//历史误差递归
    //横滚0、俯仰1、油门2、偏航3
    ctrl[1] = out;//俯仰控制值被赋值为输出值
    return;

  default://手动模式及AOA模式
    //输入数据，横滚0、俯仰1、油门2、偏航3//横滚速率0、俯仰速率1、AOA 2、横滚角3、俯仰角4
    //俯仰手动及AOA模式，直接将中间变量赋值为输入值，不需要使用比例缩放
    out = input[1];//输出值为输入1
    e_2 = e_1; e_1 = 0;//误差递归，上次误差归零
    mid = out;//中间变量被赋值为输出值
    return;

  case 3://双闭环模式-PITCHRATEPID-PITCHRATEPID-PITCHRATEPID-PITCHRATEPID-PITCHRATEPID-PITCHRATEPID-PITCHRATEPID-PITCHRATEPID-PITCHRATEPID
    //输入数据，横滚0、俯仰1、油门2、偏航3//横滚速率0、俯仰速率1、AOA 2、横滚角3、俯仰角4
    //俯仰速率闭环模式，输入为input1，反馈为俯仰速率1，使用比率缩放，将正输入值缩放到pRate倍
    if (input[1] > 0)
    {//如果输入值大于0
      e = input[1] * pRate - feedback[1];//将输入1放大到pRate倍，随后根据反馈1计算误差
    }
    else
    {//如果小于等于0
      e = input[1] - feedback[1];//直接根据输入1和反馈1计算误差
    }
    //P：0.33，I:0.11，D:0.02
    out = clamp(out + (e - e_1) / 3 + e / 9 + (e - 2 * e_1 + e_2) / 45, -4096, 4096);//使用增量式PID计算输出值
    e_2 = e_1; e_1 = e;//历史误差递归
    mid = out;//中间变量被赋值为输出值
    return; 
  }
}

void PID_AOA(void)
{
  static int32_t out, e_1, e_2;//建立输出静态变量、上次误差、上上次误差静态变量
  int32_t e;
  switch (flightMode)
  {//根据飞行模式调整PID策略
  case 0://自稳模式
    //自稳模式不由AOA控制器负责，直接跳出
    return;

  case 1://手动模式
    //输入数据，横滚0、俯仰1、油门2、偏航3//横滚速率0、俯仰速率1、AOA 2、横滚角3、俯仰角4
    //攻角手动模式，输入为mid，不需要计算，直接输出
    out = mid;
    e_2 = e_1; e_1 = 0;//历史误差递归，上次误差归零
    //横滚0、俯仰1、油门2、偏航3
    ctrl[1] = out;//俯仰控制值赋值为输出值
    return;

  default://AOA模式-AOAPID-AOAPID-AOAPID-AOAPID-AOAPID-AOAPID-AOAPID-AOAPID-AOAPID-AOAPID-AOAPID-AOAPID-AOAPID-AOAPID-AOAPID-AOAPID
    //输入数据，横滚0、俯仰1、油门2、偏航3//横滚速率0、俯仰速率1、AOA 2、横滚角3、俯仰角4
    //AOA模式，输入为中间变量，反馈为AOA 2，使用比率缩放，将正输入值缩放到pRate倍
    if (mid > 0)
    {//如果中间变量大于0
      e = mid * pRate - feedback[2];//将中间变量放大到pRate倍，随后根据反馈2计算误差
    }
    else
    {//如果小于等于0
      e = mid - feedback[2];//直接根据中间变量和反馈2计算误差
    }
    //P：0.7，I:0.0，D:0.0
    out = clamp(out + (e - e_1) / 5 * 7 / 2 + e / 10 + (e - 2 * e_1 + e_2) / 100, -4096, 4096);//使用增量式PID计算输出值
    e_2 = e_1; e_1 = e;//历史误差递归
    //横滚0、俯仰1、油门2、偏航3
    ctrl[1] = out;//俯仰控制值赋值为输出值
    return; 
  }
}

void setup()
{
  INIT_IBUS();//初始化IBUS
  INIT_IMU();//初始化IMU板
  INIT_AOAI();//初始化攻角指示器
  INIT_JY61();//初始化JY61陀螺仪
  INIT_PWM();//初始化PWM
  INIT_CTIMER();//初始化计算节拍定时器

  unsigned long dt[8] = {1500,400,400,1,1,1,1,1};//建立喂狗时间
  INIT_DOG(dt);//初始化修狗
}

void INIT_CTIMER(void)
{//初始化计算节拍定时器
  TCCR2A = 0;
  TCCR2B = 0b111;//1024分频
  TCNT2 = 143;//超调143个单位脉冲
  TIMSK2 = 0b1;//启动定时器
  //T = 0.028672s
}

ISR(TIMER2_OVF_vect)
{//TIMER2用于控制计算节拍
  TCNT2 = 143;//溢出后超调143个单位脉冲
  calcState ++;//计算阶段标志进1
  calcState = calcState % 4;//保持周期4以内
  calcFlag = 1;//计算更新标志置1
}

void loop()
{
  uint8_t serialRam = clamp(Serial.available(),0,16);//串口缓存数据变量，IBUS是32字节数据，先取一半
  for (uint8_t i = 0; i < serialRam; i ++)
  {
    IBUS_DECODE(Serial.read());//解算函数
  }
  if (IBUS_UPDATED())
  {//如果检测到一次IBUS更新
    DOG_FEED(0);//喂狗
    input[0] = map(IBUS_VAL(0),988,2012,-4096,4096);//更新输入0
    input[1] = map(IBUS_VAL(1),988,2012,-4096,4096);//更新输入1
    input[2] = map(IBUS_VAL(2),988,2012,-4096,4096);//更新输入2
    //input[3] = map(IBUS_VAL(3),988,2012,-4096,4096);//更新输入3
    if (IBUS_VAL(5) < 1300)
    {//检测第6通道数据，低于1300手动模式，低于1700攻角模式，其余双闭环模式
      flightMode = 1;
    }
    else if (IBUS_VAL(5) < 1700)
    {
      flightMode = 2;
    }
    else
    {
      flightMode = 3;
    }
    if (IBUS_VAL(4) > 1500)
    {//检测第5通道，高于1500自稳
      flightMode = 0;
    }
  }

  serialRam = clamp(IMU_AVAILABLE(SC16IS752_CHANNEL_B),0,6);//JY61是11字节数据，先取一半
  for (uint8_t i = 0; i < serialRam; i ++)
  {
    JY61_RX(IMU_READ(SC16IS752_CHANNEL_B));//解算函数
  }
  if (JY61_UPDATED())
  {//检测到一次完整的更新
    DOG_FEED(2);//喂狗
    feedback[0] = JY61_W1();//横滚角速率更新
    feedback[1] = JY61_W0();//俯仰角速率更新
    feedback[3] = JY61_THETA1();//横滚角更新
    feedback[4] = JY61_THETA0();//俯仰角更新
  }
  
  serialRam = clamp(IMU_AVAILABLE(SC16IS752_CHANNEL_A),0,5);//编码器是9字节数据，先取一半
  for (uint8_t i = 0; i < serialRam; i ++)
  {
    AOAI_RX(IMU_READ(SC16IS752_CHANNEL_A));//解算函数
  }
  if (AOAI_UPDATED())
  {
    DOG_FEED(1);//喂狗
    feedback[2] = AOAI_AOA();//攻角更新
  }

  if (DOG_CODE() & 0b1)
  {//失控判断
    input[0] = 0;
    input[1] = 0;
    input[2] = -4096;
    input[3] = 0;
    flightMode = 0;
  }

  if (calcFlag)
  {//如果计算更新标志位更新
    ctrl[2] = input[2];//油门等于输入
    ctrl[3] = 0;//偏航等于0
    switch (calcState)
    {
    case 0://横滚计算项
      PID_ROL();
      Serial.print(millis());//时间A
      Serial.print(",");
      Serial.print(input[0]/32);//横滚B
      Serial.print(",");
      break;

    case 1://俯仰计算项
      PID_PIT();
      Serial.print(feedback[0]/32);//横滚速率C
      Serial.print(",");
      Serial.print(input[1]/32);//俯仰D
      Serial.print(",");
      Serial.print(mid/32);//中间指令E
      Serial.print(",");
      break;

    case 2://攻角计算项
      PID_AOA();
      Serial.print(feedback[1]/32);//俯仰速率F
      Serial.print(",");
      Serial.print(feedback[2]/32);//AOA G
      Serial.print(",");
      Serial.print(flightMode);//飞行模式H
      Serial.print(",");
      break;

    case 3://混控计算项
      MIXER(ctrl);
      int32_t out[6] = {};
      out[0] = MIXER_VAL(0);
      out[1] = MIXER_VAL(1);
      out[2] = MIXER_VAL(2);
      out[3] = MIXER_VAL(3);
      out[4] = MIXER_VAL(4);
      out[5] = MIXER_VAL(5);
      PWM_VAL(out);
      Serial.print(input[2]/32);//油门I
      Serial.println(",");
      break;
    }
    calcFlag = 0;//复位计算更新标志位
  }
}
